#ifndef PID_H
#define PID_H

#include <algorithm/filter.h>
#include <algorithm>
#include <utility>
#include <clock.h>
#include <interface.h>

namespace detail {
/**
 * @brief 积分器基类
 *
 * 仅作规范接口使用，不用做动态多态
 */
class BaseI {
public:
    virtual ~BaseI() = default;
    virtual void reset() = 0;
    virtual float update(float ref, float fdb, float ki, float dt_s) = 0;
};

/**
 * @brief 微分器基类
 *
 * 仅作规范接口使用，不用做动态多态
 */
class BaseD {
public:
    virtual ~BaseD() = default;
    virtual void reset() = 0;
    virtual float update(float ref, float fdb, float kd, float dt_s) = 0;
};
}  // namespace detail

namespace os::pid {
/**
 * @brief 标准积分器
 * 积分限幅
 * 梯形积分
 */
class StandardI final : public detail::BaseI {
    float sum_{};
    float sumMax_{};
    float last_err_{};

public:
    explicit StandardI(float const sumMax) : sumMax_(sumMax) {}
    void reset() override { sum_ = 0.0f; }
    float update(float const ref, float const fdb, float const ki, float const dt_s) override {
        float const ei = 0.5f * (ref - fdb + std::exchange(last_err_, ref - fdb));
        sum_ += ki * ei * dt_s;
        return sum_ = std::clamp(sum_, -sumMax_, sumMax_);
    }
};

/**
 * @brief 变速积分器
 * @tparam ScaleFn err-scale变速曲线
 * Example scaleFn : auto scaleFn = [](float const err) -> float { return err < 20 ? 1.f : 0.f; };
 */
template <typename ScaleFn>
class VariableSpeedI final : public detail::BaseI {
    float sum_{};
    float sumMax_{};
    ScaleFn scale_;

public:
    VariableSpeedI(float const sumMax, ScaleFn scale) : sumMax_(sumMax), scale_(std::move(scale)) {
        static_assert(std::is_invocable_r_v<float, ScaleFn, float>,
                      "ScaleFn must be callable as float(float) and return float");
    }

    void reset() override { sum_ = 0.0f; }

    float update(float const ref, float const fdb, float const ki, float const dt_s) override {
        float g = scale_(ref - fdb);
        if (!std::isfinite(g)) g = 0.0f;

        sum_ += ki * g * (ref - fdb) * dt_s;
        return sum_ = std::clamp(sum_, -sumMax_, sumMax_);
    }
};

/**
 * @brief 标准微分器
 */
class StandardD final : public detail::BaseD {
public:
    StandardD() = default;
    void reset() override { last_error_ = 0.0f; }
    float update(float const ref, float const fdb, float const kd, float const dt_s) override {
        return kd * (ref - fdb - std::exchange(last_error_, ref - fdb)) / dt_s;
    }
    float last_error_ = 0.0f;
};

/**
 * @brief 低通滤波微分器
 * @tparam order 滤波器阶数
 * @tparam calculate_freq pid计算频率
 * @tparam cutoff_freq 滤波器截止频率
 */
template <size_t order, size_t calculate_freq, size_t cutoff_freq>
class FilteredD final : public detail::BaseD {
    using Filter = filter::IIRLowPassFilter<order, calculate_freq, cutoff_freq>;
    using Coef = typename Filter::CoefficientArray;

    Filter lpf_{};
    float prev_ef_{};
    bool inited_{false};

public:
    // b,a 可由 MATLAB 计算: [b,a] = butter(order, cutoff_freq/(calculate_freq/2))
    FilteredD(Coef const& b, Coef const& a) : lpf_(b, a) {}
    // 自动计算a,b，参数稳定后建议用第一个构造函数
    FilteredD() = default;

    void reset() override {
        lpf_.reset();
        prev_ef_ = 0.0f;
        inited_ = false;
    }

    float update(float const ref, float const fdb, float const kd, float const dt_s) override {
        float const ef = lpf_.calculate(ref - fdb);

        float deriv = 0.0f;
        if (!inited_) {
            inited_ = true;
        } else {
            deriv = (ef - prev_ef_) / dt_s;
        }
        prev_ef_ = ef;

        return kd * deriv;
    }
};

/**
 * @brief 微分先行适配器
 * @tparam D 适配者
 */
template <typename D>
class DOMAdapter final : public detail::BaseD {
    static_assert(std::is_base_of_v<BaseD, D>, "InnerD must derive from BaseD");
    D inner_;

public:
    explicit DOMAdapter(D inner = D{}) : inner_(std::move(inner)) {}

    void reset() override { inner_.reset(); }

    // 使用微分先行：只对 fdb 做微分
    float update(float /*ref*/, float fdb, float kd, float dt_s) override { return inner_.update(0.0f, fdb, kd, dt_s); }

    D& inner() { return inner_; }
    D const& inner() const { return inner_; }
};

/**
 * @brief pid控制器
 * @tparam IType 积分器类型
 * @tparam DType 微分器类型
 */
template <typename IType = StandardI, typename DType = StandardD>
class PID final : public CloseLoopController<float, float> {
    static_assert(std::is_base_of_v<detail::BaseI, IType>);
    static_assert(std::is_base_of_v<detail::BaseD, DType>);

    float kp_, ki_, kd_;
    uint64_t last_update_time{};
    float output_{};
    float outputMax_;
    float ref_{};
    float fdb_{};
    IType integrator_;
    DType differentiator_;

public:
    PID(float const kp, float const ki, float const kd, float const outputMax = 1e6, IType i = IType(),
        DType d = DType())
        : kp_(kp), ki_(ki), kd_(kd), outputMax_(outputMax), integrator_(i), differentiator_(d) {}

    void setReference(float const& ref) override { ref_ = ref; }
    void setFeedback(float const& fb) override { fdb_ = fb; }

    float calculate() override {
        auto const t_us = getDWTTime_i64();
        auto const dt_s = static_cast<float>(t_us - std::exchange(last_update_time, t_us)) / 1000000.0f;

        return output_ = std::clamp(kp_ * (ref_ - fdb_) + integrator_.update(ref_, fdb_, ki_, dt_s) +
                                        differentiator_.update(ref_, fdb_, kd_, dt_s),
                                    -outputMax_, outputMax_);
    }

    [[nodiscard]] float getOutput() const override { return output_; }

    void reset() override {
        integrator_.reset();
        differentiator_.reset();
        ref_ = 0;
        fdb_ = 0;
        output_ = 0;
        last_update_time = getDWTTime_i64();
    }

    void setKp(float const kp) { kp_ = kp; }
    void setKi(float const ki) { ki_ = ki; }
    void setKd(float const kd) { kd_ = kd; }
    void setOutputMax(float const outputMax) { outputMax_ = outputMax; }
    IType& integrator() { return integrator_; }
    DType& differentiator() { return differentiator_; }
};

enum { NB = 0, NM = 1, NS = 2, ZO = 3, PS = 4, PM = 5, PB = 6 };

/**
 * @brief 模糊pid控制器
 * @tparam IType 积分器
 * @tparam DType 微分器
 */
template <typename IType = StandardI, typename DType = StandardD>
class FuzzyPID final : public CloseLoopController<float, float> {
public:
    using Interval = struct {
        float32_t left;
        float32_t right;
    };

private:
    float32_t kp_{};
    float32_t ki_{};
    float32_t kd_{};

    uint8_t const (*kpRule_)[7]{nullptr};
    uint8_t const (*kiRule_)[7]{nullptr};
    uint8_t const (*kdRule_)[7]{nullptr};

    float32_t const* kpSet_{nullptr};
    float32_t const* kiSet_{nullptr};
    float32_t const* kdSet_{nullptr};

    Interval normalErrorRange_;
    Interval normalErrorChangeRange_;

    float32_t output_{};
    float32_t outputMax_{};  // out limit

    float32_t ref_{};
    float32_t fdb_{};

    float32_t err_{};
    float32_t lastErr_{};

    uint64_t lastUpdateTime_{};  // us

    IType integrator_;
    DType differentiator_;

public:
    FuzzyPID(uint8_t const (*kpRule)[7], uint8_t const (*kiRule)[7], uint8_t const (*kdRule)[7],
             float32_t const kpSet[7], float32_t const kiSet[7], float32_t const kdSet[7],
             Interval const& normalErrorRange, Interval const& normalErrorChangeRange, float const outputMax = 1e6,
             IType i = IType(), DType d = DType())
        : kpRule_{kpRule},
          kiRule_{kiRule},
          kdRule_{kdRule},
          kpSet_{kpSet},
          kiSet_{kiSet},
          kdSet_{kdSet},
          normalErrorRange_{normalErrorRange},
          normalErrorChangeRange_{normalErrorChangeRange},
          outputMax_{outputMax},
          integrator_(i),
          differentiator_(d) {}

    static float32_t tableLookup(uint8_t const (*rule)[7], float32_t const* set, Interval const& eRange,
                                 Interval const& ecRange, float32_t const& e, float32_t const& ec) {
        float32_t&& mappedE = 3 * ((2 * e - eRange.right - eRange.left) / (eRange.right - eRange.left));
        float32_t&& mappedEC = 3 * ((2 * ec - ecRange.right - ecRange.left) / (ecRange.right - ecRange.left));
        uint8_t eIndex1, eIndex2;
        float32_t eWeight;
        uint8_t ecIndex1, ecIndex2;
        float32_t ecWeight;
        if (mappedE < -3) {
            eIndex1 = NB;
            eIndex2 = NB;
            eWeight = 0;
        } else if (mappedE < -2) {
            eIndex1 = NB;
            eIndex2 = NM;
            eWeight = mappedE + 3;
        } else if (mappedE < -1) {
            eIndex1 = NM;
            eIndex2 = NS;
            eWeight = mappedE + 2;
        } else if (mappedE < 0) {
            eIndex1 = NS;
            eIndex2 = ZO;
            eWeight = mappedE + 1;
        } else if (mappedE < 1) {
            eIndex1 = ZO;
            eIndex2 = PS;
            eWeight = mappedE;
        } else if (mappedE < 2) {
            eIndex1 = PS;
            eIndex2 = PM;
            eWeight = mappedE - 1;
        } else if (mappedE < 3) {
            eIndex1 = PM;
            eIndex2 = PB;
            eWeight = mappedE - 2;
        } else {
            eIndex1 = PB;
            eIndex2 = PB;
            eWeight = 0;
        }

        if (mappedEC < -3) {
            ecIndex1 = NB;
            ecIndex2 = NB;
            ecWeight = 0;
        } else if (mappedEC < -2) {
            ecIndex1 = NB;
            ecIndex2 = NM;
            ecWeight = mappedEC + 3;
        } else if (mappedEC < -1) {
            ecIndex1 = NM;
            ecIndex2 = NS;
            ecWeight = mappedEC + 2;
        } else if (mappedEC < 0) {
            ecIndex1 = NS;
            ecIndex2 = ZO;
            ecWeight = mappedEC + 1;
        } else if (mappedEC < 1) {
            ecIndex1 = ZO;
            ecIndex2 = PS;
            ecWeight = mappedEC;
        } else if (mappedEC < 2) {
            ecIndex1 = PS;
            ecIndex2 = PM;
            ecWeight = mappedEC - 1;
        } else if (mappedEC < 3) {
            ecIndex1 = PM;
            ecIndex2 = PB;
            ecWeight = mappedEC - 2;
        } else {
            ecIndex1 = PB;
            ecIndex2 = PB;
            ecWeight = 0;
        }

        return set[rule[eIndex1][ecIndex1]] * (1 - eWeight) * (1 - ecWeight) +
               set[rule[eIndex1][ecIndex2]] * (1 - eWeight) * ecWeight +
               set[rule[eIndex2][ecIndex1]] * eWeight * (1 - ecWeight) +
               set[rule[eIndex2][ecIndex2]] * eWeight * ecWeight;
    }

    void setReference(float const& ref) override { ref_ = ref; }

    void setFeedback(float const& fdb) override { fdb_ = fdb; }

    float calculate() override {
        auto const t_us = getDWTTime_i64();
        auto const dt_s = static_cast<float>(t_us - std::exchange(lastUpdateTime_, t_us)) / 1000000.0f;

        // Calculate the differential value
        err_ = ref_ - fdb_;
        float32_t const dError = err_ - lastErr_;
        float32_t const ec = dError / dt_s;
        kp_ = tableLookup(kpRule_, kpSet_, normalErrorRange_, normalErrorChangeRange_, err_, ec);
        ki_ = tableLookup(kiRule_, kiSet_, normalErrorRange_, normalErrorChangeRange_, err_, ec);
        kd_ = tableLookup(kdRule_, kdSet_, normalErrorRange_, normalErrorChangeRange_, err_, ec);

        return output_ = std::clamp(kp_ * err_ + integrator_.update(ref_, fdb_, ki_, dt_s) +
                                        differentiator_.update(ref_, fdb_, kd_, dt_s),
                                    -outputMax_, outputMax_);
    }

    void reset() override {
        integrator_.reset();
        differentiator_.reset();
        ref_ = 0;
        fdb_ = 0;
        err_ = 0;
        lastErr_ = 0;
        output_ = 0;
        lastUpdateTime_ = getDWTTime_i64();
    }

    [[nodiscard]] float32_t getError() const { return err_; }

    [[nodiscard]] float32_t getOutput() const override { return output_; }

    [[nodiscard]] float32_t getFeedback() const { return fdb_; }

    [[nodiscard]] float getRef() const { return ref_; }

    ~FuzzyPID() override = default;
};
}  // namespace os::pid

#endif
